#include "RigidBodyFriction.h"

namespace coolpool
{
	void addFrictionToImpulse(const Collision * collision, RigidBody * rigid_body1, RigidBody * rigid_body2, double normal_impulse_size)
	{
		// Vectors from center to the collision points.
		math_tools::Vector3D r1 = collision->collision_point - rigid_body1->getCenterOfMassLocation();
		math_tools::Vector3D r2 = collision->collision_point - rigid_body2->getCenterOfMassLocation();

		// Collision point relative velocity
		math_tools::Vector3D col_point_vel_rel = rigid_body1->pointVelocity(collision->collision_point);
		if (!rigid_body2->isStatic())
			col_point_vel_rel -= rigid_body2->pointVelocity(collision->collision_point);

		// Relative tangent velocity.
		math_tools::Vector3D tangent_vel = col_point_vel_rel - (collision->collision_normal * col_point_vel_rel.dot(collision->collision_normal));
		double tangent_speed = tangent_vel.mag();

		// If the tangent speed_is not zero, drag friction occurs.
		if (!equalsZero(tangent_speed))
		{
			math_tools::Vector3D T = -tangent_vel * (1 / tangent_speed);

			math_tools::Vector3D r1_cross_col_tangent = r1 * T;
			matrix_tools::transform(rigid_body1->getInertiaTensorInverse(), r1_cross_col_tangent, r1_cross_col_tangent);
			double denominator = (1.0 / rigid_body1->getMass()) + T.dot(r1_cross_col_tangent * r1);

			if (!rigid_body2->isStatic())
			{
				math_tools::Vector3D r2_cross_col_tangent = r2 * T;
				matrix_tools::transform(rigid_body2->getInertiaTensorInverse(), r2_cross_col_tangent, r2_cross_col_tangent);
				denominator += (1.0 / rigid_body2->getMass()) + T.dot(r2_cross_col_tangent * r2);
			}

			if (denominator > EPSILON)
			{
				// Impulse to reverse is the force that is needed to make the relative tangent velocity zero.
				double impulse_to_reverse = tangent_speed / denominator;
				// Maximum impulse which can occur from static friction.
				double impulse_from_normal_impulse = ((normal_impulse_size) * rigid_body1->getCoefficientOfStaticFrictionWith(rigid_body2->getType()));

				// If impulse_to_reverse is smaller it means that the static friction was not overcomed, so the negative of with will be applied to the body,
				// otherwise dynamic friction will be applied.
				double friction_impulse = (impulse_to_reverse < impulse_from_normal_impulse) ? impulse_to_reverse : (normal_impulse_size * rigid_body1->getCoefficientOfDynamicFrictionWith(
						rigid_body2->getType()));

				T *= friction_impulse;
				rigid_body1->setLinearVelocity(rigid_body1->getLinearVelocity() + (T * (1.0 / rigid_body1->getMass())));
				rigid_body1->setAngularMomentum(rigid_body1->getAngularMomentum() + (r1 * T));

				if (!rigid_body2->isStatic())
				{
					rigid_body2->setLinearVelocity(rigid_body2->getLinearVelocity() + (T * (1.0 / rigid_body2->getMass())));
					rigid_body2->setAngularMomentum(rigid_body2->getAngularMomentum() + (r2 * T));
				}
			}
		}
		else // If the tangent speed is zero rolling resistance occurs.
		{
			// Currently implemented only for spheres.
			if (rigid_body1->getType() == CP_BALL && rigid_body2->getType() == CP_REC)
			{
				addRollingFriction(static_cast<Ball *> (rigid_body1), collision->collision_normal, r1, rigid_body2, normal_impulse_size);
			}
			else
			{
				// TODO: New objects.
			}
		}
	}

	void addRollingFriction(Ball * ball, const math_tools::Vector3D & collision_normal, const math_tools::Vector3D & ball_r, RigidBody * surface, double normal_impulse_size)
	{
		// Rolling friction size according the formula:
		double rolling_friction_size = normal_impulse_size * ball->getCoefficientOfRollingFrictionWith(surface->getType()) / ball->getRadius();

		// It needs to be applied, against the tangential velocity of the center.
		math_tools::Vector3D tangent_vel = ball->getLinearMomentum() - (collision_normal * ball->getLinearMomentum().dot(collision_normal));
		math_tools::Vector3D linear_part = -tangent_vel;
		linear_part = linear_part.unit() * (rolling_friction_size);
		ball->addForce(linear_part);

		// TODO: Not sure if this is the correct way how to apply the the angular friction force, but it looks nice :)
		math_tools::Vector3D angular_part = -ball->getAngularMomentum();
		angular_part.unit();
		angular_part *= rolling_friction_size;
		matrix_tools::transform(ball->getInertiaTensor(), angular_part, angular_part);
		ball->addTorque(angular_part);
	}
}
